#include "rclcpp/rclcpp.hpp"  
#include "sensor_msgs/msg/image.hpp"  
#include "origincar_msg/msg/sign.hpp"  
#include "ai_msgs/msg/perception_targets.hpp"  
#include <opencv2/opencv.hpp>  
#include <cv_bridge/cv_bridge.h>  
#include <pyzbar/pyzbar.h>  

class QrcodeNode : public rclcpp::Node {  
public:  
    QrcodeNode() : Node("qrcode"), latest_roi_{0, 0, 0, 0} {  
        publisher_qrcode_ = this->create_publisher<origincar_msg::msg::Sign>("sign_switch", 10);  
        subscription_ = this->create_subscription<sensor_msgs::msg::Image>(  
            "/image",  
            std::bind(&QrcodeNode::image_callback, this, std::placeholders::_1)  
        );  
        subscription_yolo_ = this->create_subscription<ai_msgs::msg::PerceptionTargets>(  
            "hobot_dnn_detection",  
            std::bind(&QrcodeNode::listener_yolo_callback, this, std::placeholders::_1)  
        );  
    }  

private:  
    // Publisher  
    rclcpp::Publisher<origincar_msg::msg::Sign>::SharedPtr publisher_qrcode_;  
    // Subscribers  
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;  
    rclcpp::Subscription<ai_msgs::msg::PerceptionTargets>::SharedPtr subscription_yolo_;  

    // Latest ROI for QR code  
    cv::Rect latest_roi_;  
    cv::Mat latest_image_;  

    void listener_yolo_callback(const ai_msgs::msg::PerceptionTargets::SharedPtr msg) {  
        for (const auto &target : msg->targets) {  
            std::string class_type = target.rois[0].type;  
            if (class_type == "qrcode") {  
                const auto &roi = target.rois[0].rect;  
                latest_roi_ = cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height);  
                RCLCPP_INFO(this->get_logger(), "检测到二维码区域，准备解码");  
                return;  // 只处理第一个检测到的二维码  
            }  
        }  
    }  

    void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) {  
        if (latest_roi_.width == 0 && latest_roi_.height == 0) {  
            return;  // 如果没有检测到二维码 ROI，跳过图像处理  
        }  

        // 将 ROS 图像消息转换为 OpenCV 图像格式  
        auto cv_image = cv_bridge::toCvShare(msg, "bgr8")->image;  

        // 从 ROI 区域裁剪图像  
        cv::Mat roi_image = cv_image(latest_roi_);  

        // 调用二维码解码函数  
        auto qr_codes = decode_qr_code(roi_image);  

        // 如果检测到二维码，发布解码结果  
        if (!qr_codes.empty()) {  
            for (const auto &obj : qr_codes) {  
                RCLCPP_INFO(this->get_logger(), "检测到二维码: %s", obj.data.c_str());  

                // 创建并发布 Sign 消息  
                origincar_msg::msg::Sign sign_msg;  
                if (obj.data == "ClockWise") {  
                    sign_msg.sign_data = 3;  
                } else if (obj.data == "AntiClockWise") {  
                    sign_msg.sign_data = 4;  
                } else {  
                    sign_msg.sign_data = 0;  
                }  
                publisher_qrcode_->publish(sign_msg);  
            }  
            // 重置 latest_roi 以避免重复检测  
            latest_roi_ = cv::Rect(0, 0, 0, 0);  
        }  
    }  

    std::vector<pyzbar::DecodedObject> decode_qr_code(const cv::Mat& image) {  
        // 使用 pyzbar 进行二维码解码  
        std::vector<pyzbar::DecodedObject> decoded_objects = pyzbar::decode(image);  
        return decoded_objects;  
    }  
};  

int main(int argc, char **argv) {  
    rclcpp::init(argc, argv);  
    rclcpp::spin(std::make_shared<QrcodeNode>());  
    rclcpp::shutdown();  
    return 0;  
}